using BepuUtilities;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.Constraints.Contact
{
    public struct PenetrationLimitProjection
    {
        // 请注意,这些只是原始的雅克比,没有使用JT*EffectiveMass值进行预计算。
        public Vector3Wide AngularA;
        public Vector3Wide AngularB;
        public Vector<float> EffectiveMass;
        public Vector<float> BiasVelocity;
    }

    public static class PenetrationLimit
    {
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Prestep(in BodyInertias inertiaA, in BodyInertias inertiaB,
            in Vector3Wide contactOffsetA, in Vector3Wide contactOffsetB, in Vector3Wide normal, in Vector<float> depth,
            in Vector<float> positionErrorToVelocity, in Vector<float> effectiveMassCFMScale, in Vector<float> maximumRecoveryVelocity,
            float inverseDt, out PenetrationLimitProjection projection)
        {
            // 因为雅克比和误差没有经过任何处理,所以我们在这里直接取前一步的数据。

            // 接触穿透约束采用以下形式：
            // 点(位置A+偏移A,N)>=点(位置B+偏移B,N)
            // 或者：
            // 点(位置A+偏移A,N)-点(位置B+偏移B,N)>=0
            // 点(位置A+偏移量A-位置B-偏移量B,N)>=0
            // 其中位置A和位置B是主体的质心位置偏移量A和偏移量B是从质心到触点的世界空间偏移量,
            // N是校准为从B指向A的单位长度矢量。(法线指向方向很重要;它会更改符号。)
            // 在实践中,我们将使用碰撞检测系统的穿透深度,而不是尝试在这里重新计算误差。

            // 因此,将法线视为常量,速度约束为：
            // 点(d/dt(位置A+偏移A-位置B-偏移B),N)>=0
            // 点(线性速度A+d/dt(偏移量A)-线性速度B-d/dt(偏移量B)),N)>=0
            // 偏移的速度由角速度定义。
            // 点(线性速度A+角度速度A x偏移量A-线性速度B-角度速度B x偏移量B),N)>=0
            // 点(线性速度A,N)+点(角速度A x偏移A,N)-点(线性速度B,N)-点(角速度B x偏移B),N)>=0
            // 使用标量三重积的属性：
            // 点(线性速度A,N)+点(偏移A x N,角度速度A)-点(线性速度B,N)-点(偏移B x N,角度速度B)>=0
            // 烘焙负面内容：
            // 点(线性速度A,N)+点(偏移A x N,角度速度A)+点(线性速度B,-N)+点(-偏移B x N,角度速度B)>=0
            // A x B=-B x A：
            // 点(线性速度A,N)+点(偏移A x N,角度速度A)+点(线性速度B,-N)+点(N x偏移B,角度速度B)>=0
            // 好了,雅克比人！
            // 线性A：N
            // 角度A：偏移A x N
            // 线性B：-N
            // 角度B：N x偏移B
            // 请注意,我们保持穿透深度不变,即使它是负数。投机性接触！
            Vector3Wide.CrossWithoutOverlap(contactOffsetA, normal, out projection.AngularA);
            Vector3Wide.CrossWithoutOverlap(normal, contactOffsetB, out projection.AngularB);

            // 有效质量
            Symmetric3x3Wide.VectorSandwich(projection.AngularA, inertiaA.InverseInertiaTensor, out var angularA0);
            Symmetric3x3Wide.VectorSandwich(projection.AngularB, inertiaB.InverseInertiaTensor, out var angularB0);

            // 线性有效质量贡献注释：
            // 1)对于线性分量,J*M^-1*JT可以重新排序为J*JT*M^-1,因为M^-1是标量,点(n*标量,n)=点(n,n)*标量。
            // 2)点(法线,法线)==1,所以每个物体的贡献就是它的反质量。
            var linear = inertiaA.InverseMass + inertiaB.InverseMass;
            // 请注意,我们不预先计算JT*EffailtiveMass项。因为雅克比人是共用的,我们无论如何都要做繁殖。
            projection.EffectiveMass = effectiveMassCFMScale / (linear + angularA0 + angularB0);

            // 如果深度为负,则偏移速度将允许向上移动,直到深度达到零。这之所以有效,是因为Position ErrorToVelocity*dt将始终<=1。
            projection.BiasVelocity = Vector.Min(
                depth * new Vector<float>(inverseDt),
                Vector.Min(depth * positionErrorToVelocity, maximumRecoveryVelocity));
        }


        /// <summary>
        /// 将冲量从约束空间变换到世界空间,并使用它修改身体的缓存世界空间速度。
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ApplyImpulse(in PenetrationLimitProjection projection, in BodyInertias inertiaA, in BodyInertias inertiaB, in Vector3Wide normal,
            in Vector<float> correctiveImpulse,
            ref BodyVelocities wsvA, ref BodyVelocities wsvB)
        {
            var linearVelocityChangeA = correctiveImpulse * inertiaA.InverseMass;
            Vector3Wide.Scale(normal, linearVelocityChangeA, out var correctiveVelocityALinearVelocity);
            Vector3Wide.Scale(projection.AngularA, correctiveImpulse, out var correctiveAngularImpulseA);
            Symmetric3x3Wide.TransformWithoutOverlap(correctiveAngularImpulseA, inertiaA.InverseInertiaTensor, out var correctiveVelocityAAngularVelocity);

            var linearVelocityChangeB = correctiveImpulse * inertiaB.InverseMass;
            Vector3Wide.Scale(normal, linearVelocityChangeB, out var correctiveVelocityBLinearVelocity);
            Vector3Wide.Scale(projection.AngularB, correctiveImpulse, out var correctiveAngularImpulseB);
            Symmetric3x3Wide.TransformWithoutOverlap(correctiveAngularImpulseB, inertiaB.InverseInertiaTensor, out var correctiveVelocityBAngularVelocity);

            Vector3Wide.Add(wsvA.Linear, correctiveVelocityALinearVelocity, out wsvA.Linear);
            Vector3Wide.Add(wsvA.Angular, correctiveVelocityAAngularVelocity, out wsvA.Angular);
            Vector3Wide.Subtract(wsvB.Linear, correctiveVelocityBLinearVelocity, out wsvB.Linear); // 注释减去;NORMAL=-JacobianLinearB
            Vector3Wide.Add(wsvB.Angular, correctiveVelocityBAngularVelocity, out wsvB.Angular);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void WarmStart(
            in PenetrationLimitProjection projection, in BodyInertias inertiaA, in BodyInertias inertiaB, in Vector3Wide normal,
            in Vector<float> accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
        {
            ApplyImpulse(projection, inertiaA, inertiaB, normal, accumulatedImpulse, ref wsvA, ref wsvB);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ComputeCorrectiveImpulse(in BodyVelocities wsvA, in BodyVelocities wsvB,
            in PenetrationLimitProjection projection,
            in Vector3Wide normal, in Vector<float> softnessImpulseScale,
            ref Vector<float> accumulatedImpulse, out Vector<float> correctiveCSI)
        {
            // 请注意,我们在这里没有使用预变换的雅克比;线性雅克比共享(法线)意味着我们无论如何都有有效质量。
            Vector3Wide.Dot(wsvA.Linear, normal, out var csvaLinear);
            Vector3Wide.Dot(wsvA.Angular, projection.AngularA, out var csvaAngular);
            Vector3Wide.Dot(wsvB.Linear, normal, out var negatedCSVBLinear);
            Vector3Wide.Dot(wsvB.Angular, projection.AngularB, out var csvbAngular);
            // 计算求反版本,以避免需要显式求反。
            var negatedCSI = accumulatedImpulse * softnessImpulseScale + (csvaLinear - negatedCSVBLinear + csvaAngular + csvbAngular - projection.BiasVelocity) * projection.EffectiveMass;

            var previousAccumulated = accumulatedImpulse;
            accumulatedImpulse = Vector.Max(Vector<float>.Zero, accumulatedImpulse - negatedCSI);

            correctiveCSI = accumulatedImpulse - previousAccumulated;

        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Solve(in PenetrationLimitProjection projection, in BodyInertias inertiaA, in BodyInertias inertiaB, in Vector3Wide normal,
            in Vector<float> softnessImpulseScale, ref Vector<float> accumulatedImpulse, ref BodyVelocities wsvA, ref BodyVelocities wsvB)
        {
            ComputeCorrectiveImpulse(wsvA, wsvB, projection, normal, softnessImpulseScale, ref accumulatedImpulse, out var correctiveCSI);
            ApplyImpulse(projection, inertiaA, inertiaB, normal, correctiveCSI, ref wsvA, ref wsvB);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void UpdatePenetrationDepth(in Vector<float> dt, in Vector3Wide contactOffsetA, in Vector3Wide offsetB, in Vector3Wide normal, in BodyVelocities velocityA, in BodyVelocities velocityB, ref Vector<float> penetrationDepth)
        {
            // 法线被校准为从B指向A。A沿N的任何移动都会导致深度减小。B沿N的任何移动都会导致深度的增加。
            // 估计深度深度变化=点(法线,速度DtA.线性+速度DtA,角度x接触点偏移量A)-点(法线,速度DtB.线性+速度DtB角度x接触点偏移量B)-点(法线,速度DtB.线性+速度DtB角度x接触点偏移量B)-点(法线,速度DtB.线性+速度DtB角度x接触点偏移量B)
            Vector3Wide.CrossWithoutOverlap(velocityA.Angular, contactOffsetA, out var wxra);
            Vector3Wide.Add(wxra, velocityA.Linear, out var contactVelocityA);

            Vector3Wide.Subtract(contactOffsetA, offsetB, out var contactOffsetB);
            Vector3Wide.CrossWithoutOverlap(velocityB.Angular, contactOffsetB, out var wxrb);
            Vector3Wide.Add(wxrb, velocityB.Linear, out var contactVelocityB);

            Vector3Wide.Subtract(contactVelocityA, contactVelocityB, out var contactVelocityDifference);
            Vector3Wide.Dot(normal, contactVelocityDifference, out var estimatedDepthChangeVelocity);
            penetrationDepth -= estimatedDepthChangeVelocity * dt;
        }
    }
}
